package nape.constraint;
import nape.phys.PhysObj;
import nape.space.Space;
import nape.constraint.ClassicCons;
import nape.constraint.Constraint;
import nape.geom.Vec2;
import nape.geom.VecMath;
import nape.util.FastMath;
import nape.Const;
















class DampedSpring extends ClassicCons {

	vec_new(public,n);
	vec_new(public,f);
	
	public var restLength:Float;
	public var stiffness:Float;
	public var damping:Float;
	
	public var damp_val:Float;
	public var nMass:Float;
	public var targ_vrn:Float;
	
	
	
	public function new(obj1:PhysObj,obj2:PhysObj,anchor1:Vec2,anchor2:Vec2,rest_length:Float,stiffness:Float,dampening:Float) {
		super(obj1,obj2,anchor1,anchor2);
		
		restLength = rest_length;
		this.stiffness = stiffness;
		damping = dampening;
	}
	
	
	
	public override function preStep(dt:Float) {
		ranchor(a1,b1,r1);
		ranchor(a2,b2,r2);
		
		rdelta(b1,b2,r1,r2,n);
		var dist; delta_norm(n,dist);

		nMass = 1.0/KScalar(b1,b2,n);
		
		var f_spring = (restLength-dist)*stiffness; 
		vec_mul(n,f_spring,f);
		if(breakable) { if(vec_lsq(f) > maxForce*maxForce) return true; }
		vec_clamp(f,maxForce);
		
		AppForce(b1,b2,r1,r2,f);
		
		targ_vrn = 0;
		damp_val = 1.0-Math.exp(-damping*dt/nMass);
		this.dt = dt;
		return false;
	}
	
	
	
	public override function applyImpulse():Bool {
		vec_new(vr); calc_relvel(b1,b2,r1,r2,vr,);
		var vrn = vec_dot(vr,n)-targ_vrn;
		
		var vdamp = -vrn*damp_val;
		targ_vrn = vrn + vdamp;
		vec_new(j); vec_mul(n,vdamp*nMass,j);
		AppImp(b1,b2,r1,r2,j,);
		return false;
	}
	
	
	
	public override function impulse(obj:PhysObj,dst:Vec2) {
		vec_new(vr); calc_relvel(b1,b2,r1,r2,vr,);
		var vrn = vec_dot(vr,n)-targ_vrn;
		
		var vdamp = -vrn*damp_val;
		targ_vrn = vrn + vdamp;
		vec_new(j); vec_mul(n,vdamp*nMass,j);
		
		if(obj==b1) { vec_mul(f,-dt,dst.p); vec_subeq(dst.p,j); }
		else      { vec_mul(f, dt,dst.p); vec_addeq(dst.p,j); }
	}
}